/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include "UPRE_MOVEBASE.h"
#include "UPRE_MOTOR.h"
#include "slowStart.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_LOCATION.h"
#include "AirCylinder.h"
#include "Task scheduling_l.h"
#include "vesc.h"
#include "UPRE_Task_Flow.h"
#include "UPRE_BSP_WiFi.h"
#include "UPRE_DIJKSTRA.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
uint8_t a = 0;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for CanSend */
osThreadId_t CanSendHandle;
const osThreadAttr_t CanSend_attributes = {
  .name = "CanSend",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for TimerTask */
osThreadId_t TimerTaskHandle;
const osThreadAttr_t TimerTask_attributes = {
  .name = "TimerTask",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for ChassicTask */
osThreadId_t ChassicTaskHandle;
const osThreadAttr_t ChassicTask_attributes = {
  .name = "ChassicTask",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for ChassicMotorTas */
osThreadId_t ChassicMotorTasHandle;
const osThreadAttr_t ChassicMotorTas_attributes = {
  .name = "ChassicMotorTas",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for ButtonScan */
osThreadId_t ButtonScanHandle;
const osThreadAttr_t ButtonScan_attributes = {
  .name = "ButtonScan",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for TopHandle */
osThreadId_t TopHandleHandle;
const osThreadAttr_t TopHandle_attributes = {
  .name = "TopHandle",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for SlowOpenTask */
osThreadId_t SlowOpenTaskHandle;
const osThreadAttr_t SlowOpenTask_attributes = {
  .name = "SlowOpenTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void StartTaskCanSend(void *argument);
void StartTimerTask(void *argument);
void StartChassicTask(void *argument);
void StartChassicMotor(void *argument);
void StartButtonScan(void *argument);
void StartTopHandle(void *argument);
void StartTaskSlowOpen(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of CanSend */
  CanSendHandle = osThreadNew(StartTaskCanSend, NULL, &CanSend_attributes);

  /* creation of TimerTask */
  TimerTaskHandle = osThreadNew(StartTimerTask, NULL, &TimerTask_attributes);

  /* creation of ChassicTask */
  ChassicTaskHandle = osThreadNew(StartChassicTask, NULL, &ChassicTask_attributes);

  /* creation of ChassicMotorTas */
  ChassicMotorTasHandle = osThreadNew(StartChassicMotor, NULL, &ChassicMotorTas_attributes);

  /* creation of ButtonScan */
  ButtonScanHandle = osThreadNew(StartButtonScan, NULL, &ButtonScan_attributes);

  /* creation of TopHandle */
  TopHandleHandle = osThreadNew(StartTopHandle, NULL, &TopHandle_attributes);

  /* creation of SlowOpenTask */
  SlowOpenTaskHandle = osThreadNew(StartTaskSlowOpen, NULL, &SlowOpenTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
    /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
extern bool flag_finsh;
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
    //0����������
    //1cdצ��
    //2abצ��
    //4����
    //5����
    /* Infinite loop */
    for (;;) {
        //      HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
        HAL_GPIO_TogglePin(LED0_GPIO_Port,LED0_Pin);
        //      HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
        //      airCylinder_all_open(&hfdcan2,0x02);
        //      airCylinder_open(&hfdcan2,0x02,5);
        osDelay(1000);
        if(flag_finsh) {
          // osDelay(15000);
            WiFi_Init();
        }
    }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartTaskCanSend */
/**
* @brief Function implementing the CanSend thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskCanSend */
void StartTaskCanSend(void *argument)
{
  /* USER CODE BEGIN StartTaskCanSend */

    /* Infinite loop */
    for (;;) {
        robotMoveTask();
        osDelay(3);
    }
  /* USER CODE END StartTaskCanSend */
}

/* USER CODE BEGIN Header_StartTimerTask */
/**
* @brief Function implementing the TimerTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTimerTask */
void StartTimerTask(void *argument)
{
  /* USER CODE BEGIN StartTimerTask */
    /* Infinite loop */
    for (;;) {
      // if(!TaskFlag.Flag_Start) {
        // osDelay(10000);
        // TaskFlag.Flag_Start=true;
        // TaskFlag.Flag_BlueZone = true;
        // Main_Control.task_state = game_begin;
      // }
        if (TaskFlag.Flag_ManualMode) {
            //     if (!Flag_ManualStart) {
            //         Flag_ManualStart = true;
            //         flag_MatchingNode = true;
            //         }
        } else {
            if (TaskFlag.Flag_Start) {
                // for (int i = 0; i < 12; ++i) {
                  // printf("%d",ball[i/6][i%6]);
                // }
                GoGetBallTask();
            }
        }
        osDelay(3);
    }
  /* USER CODE END StartTimerTask */
}

/* USER CODE BEGIN Header_StartChassicTask */
/**
* @brief Function implementing the ChassicTask thread.
* @param argument: Not u/sed
* @retval None
*/
/* USER CODE END Header_StartChassicTask */
void StartChassicTask(void *argument)
{
  /* USER CODE BEGIN StartChassicTask */
    /* Infinite loop */
    for (;;) {
        Encoder_On();
        cal_location();
        osDelay(3);
    }
  /* USER CODE END StartChassicTask */
}

/* USER CODE BEGIN Header_StartChassicMotor */
/**
* @brief Function implementing the ChassicMotorTas thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartChassicMotor */
void StartChassicMotor(void *argument)
{
  /* USER CODE BEGIN StartChassicMotor */
    /* Infinite loop */
    for (;;) {
        //      sendUltrasonic(0x11);
        //      osDelay(5);
        //      sendUltrasonic(0x12);
      // airCylinder_all_open(&hfdcan2,0x02);
        TaskExecution();
        osDelay(5);

        //      HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
    }
  /* USER CODE END StartChassicMotor */
}

/* USER CODE BEGIN Header_StartButtonScan */
/**
* @brief Function implementing the ButtonScan thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartButtonScan */
void StartButtonScan(void *argument)
{
  /* USER CODE BEGIN StartButtonScan */
    /* Infinite loop */
    for (;;) {
        Task_change();
        osDelay(2);
        //      Motor_param_set.claw_grain_left_dji3508_pos = 150.f;
        //      Motor_param_set.claw_grain_right_dji3508_pos = -149.f;
        //      Motor_param_set.claw_storage_left_dji2006_pos = -94.f;
        //      Motor_param_set.claw_storage_right_dji2006_pos = 96.f;
        //      Motor_param_set.claw_pick_ball_grasp_dji2006_pos=-170;
        //      osDelay(2000);
        //      Motor_param_set.claw_pick_ball_grasp_dji2006_pos=-35;
        //      osDelay(2000);
        //      Motor_param_set.claw_pick_ball_overturn_dji2006_pos = 200.f;//TODO�м�λ�ã���΢����
        //      Motor_param_set.claw_pick_ball_overturn_dji2006_pos = 450.f;//TODOǰȡ��ˮƽλ��(��΢��)
        //      Motor_param_set.shoot_ball_behind_dji2006_vel = 5000.f;//�����?
    }
  /* USER CODE END StartButtonScan */
}

/* USER CODE BEGIN Header_StartTopHandle */
/**
* @brief Function implementing the TopHandle thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTopHandle */
void StartTopHandle(void *argument)
{
  /* USER CODE BEGIN StartTopHandle */
    /* Infinite loop */
    for (;;) {
      if (TaskFlag.Flag_BlueZone && !TaskFlag.Flag_RedZone) {
        Air_Cylinder_Control_blue();
      }else if(TaskFlag.Flag_RedZone && !TaskFlag.Flag_BlueZone) {
        Air_Cylinder_Control_red();
      }
        Control_Motor();
        osDelay(3);
    }
  /* USER CODE END StartTopHandle */
}

/* USER CODE BEGIN Header_StartTaskSlowOpen */
/**
* @brief Function implementing the SlowOpenTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskSlowOpen */
void StartTaskSlowOpen(void *argument)
{
  /* USER CODE BEGIN StartTaskSlowOpen */
  /* Infinite loop */
  for(;;)
  {
    slowStart_allopen(&hfdcan2, 0x02);
    osDelay(200);
  }
  /* USER CODE END StartTaskSlowOpen */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

